﻿using System;
using System.Collections.Generic;
using System.Text;
using FIRADriverLibrary;
using System.Collections;
using GeometryCSharp;
using System.Drawing;

namespace AGHFiraPlayer.Drivers
{
    public abstract class GoalKeeperDriver
    {
        public static ArrayList angleLocks;
        public static Point2D checkPoint;

        public const double wheelsHalfDistance = 0.034;
        public const double robotCollisionAvoidanceDistance = 0.2;
        public const double speedUpDistance = 0.2;

        public static double tinyVelocity = 0.2;	//below robot will change direction if needed, above will turn
        public static double freeCurveFactor = 0.6;
        public static double collisionDetectionTime = 0.5;

        public static void DriveVelocity(FIRARobotState robot, double x, double y, double velocity)
        {
            double linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;
            double deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;

            checkPoint = CalculateCollisionFreeCheckPoint(robot, x, y, Math.Abs(collisionDetectionTime * linearVel) + robotCollisionAvoidanceDistance);
            linearVel = (robot.leftSpeed + robot.rightSpeed) / 2;	//might have chaged
            deltaVel = (robot.leftSpeed - robot.rightSpeed) / 2;
            if (checkPoint.x != 0.0 || checkPoint.y != 0.0)
            {
                CalculateControlNoObstacles(robot, checkPoint.x, checkPoint.y, velocity, ref linearVel, ref deltaVel);
            }
            else
            {
                CalculateControlNoObstacles(robot, x, y, velocity, ref linearVel, ref deltaVel);
                angleLocks.Clear();
            }

            robot.leftSpeed = linearVel + deltaVel;
            robot.rightSpeed = linearVel - deltaVel;
        }

        public static void CalculateControlNoObstacles(FIRARobotState robot, double x, double y, double velocity, ref double linearVel, ref double deltaVel)
        {
            Vector2D robotVersor = new Vector2D(robot.angle);
            Vector2D targetVector = new Vector2D(x - robot.x, y - robot.y);

            double angleRobotTarget = Vector2D.AngleBetween(robotVersor, targetVector);
            linearVel = Math.Sign(Math.Cos(angleRobotTarget)) * Math.Min(PotencialDriver.maxVelocity, velocity);
            deltaVel = freeCurveFactor * Math.Sin(angleRobotTarget) * linearVel;
            if (targetVector.Length < robotCollisionAvoidanceDistance)	// sharpen turn if close to target
            {
                double sharpenFactor = (robotCollisionAvoidanceDistance - targetVector.Length) / robotCollisionAvoidanceDistance;
                deltaVel *= 1 + sharpenFactor;
                linearVel *= 0.5 + 0.5 * (1 - sharpenFactor);
            }

            if (Math.Abs(robot.leftSpeed + robot.rightSpeed) / 2 > tinyVelocity)	// robot at motion
            {
                if (Math.Sign(robot.leftSpeed + robot.rightSpeed) != Math.Sign(linearVel))	// invert motion
                {
                    linearVel = -linearVel;
                    deltaVel = Math.Sign(Math.Sin(angleRobotTarget)) * freeCurveFactor * linearVel;
                }
            }
        }

        private static Point2D CalculateCollisionFreeCheckPoint(FIRARobotState robot, double x, double y, double lookupDistance)
        {
            //			Vector2D robotVector = new Vector2D(robot.angle);
            //			double robotAngle = robot.angle * Math.Sign(robot.leftSpeed + robot.rightSpeed);

            Vector2D targetVector = new Vector2D(x - robot.x, y - robot.y);
            Vector2D robotVersor = new Vector2D(robot.angle);
            double lookupDistanceVectorLength = lookupDistance * Math.Sign(robot.leftSpeed + robot.rightSpeed);
            Vector2D robotVector = targetVector + (robotVersor * lookupDistanceVectorLength);
            double robotAngle = robotVector.Angle;

            angleLocks = new ArrayList();
            Vector2D robotObstacleRobotVector;

            // robots locks
            foreach (FIRARobotState obstacleRobot in Predicator.firaDriver.firaRobots)
            {
                if (obstacleRobot == robot)
                    continue;

                robotObstacleRobotVector = new Vector2D(obstacleRobot.x - robot.x, obstacleRobot.y - robot.y);
                double obstacleDistance = robotObstacleRobotVector.Length;
                if (obstacleDistance > lookupDistance)
                    continue;

                double obstacleAngle = robotObstacleRobotVector.Angle;
                double obstacleWidthHalfAngle = Math.Atan(robotCollisionAvoidanceDistance / obstacleDistance);
                if (obstacleDistance < robotCollisionAvoidanceDistance)
                    obstacleWidthHalfAngle *= 1 + (robotCollisionAvoidanceDistance - obstacleDistance) / robotCollisionAvoidanceDistance;
                angleLocks.Add(new AngleLock(obstacleAngle, obstacleWidthHalfAngle, obstacleDistance));
            }

            //walls locks
            double wallDistance = Math.Max(0.01, robot.x - Predicator.pitchArea.Left);
            if (wallDistance > 0.0 && wallDistance < lookupDistance / 3)
            {
                angleLocks.Add(new AngleLock(Math.PI, Math.Acos(wallDistance / lookupDistance), wallDistance));
            }
            wallDistance = Math.Max(0.01, Predicator.pitchArea.Right - robot.x);
            if (wallDistance > 0.0 && wallDistance < lookupDistance / 3)
            {
                angleLocks.Add(new AngleLock(0.01, Math.Acos(wallDistance / lookupDistance), wallDistance));
            }
            wallDistance = Math.Max(0.01, robot.y - Predicator.pitchArea.Top / 3);
            if (wallDistance < lookupDistance)
            {
                angleLocks.Add(new AngleLock(3 * Math.PI / 2, Math.Acos(wallDistance / lookupDistance), wallDistance));
            }
            wallDistance = Math.Max(0.01, Predicator.pitchArea.Bottom - robot.y / 3);
            if (wallDistance < lookupDistance)
            {
                angleLocks.Add(new AngleLock(Math.PI / 2, Math.Acos(wallDistance / lookupDistance), wallDistance));
            }

            //remove too far locks, stop if a very close lock is present
            for (int i = 0; i < angleLocks.Count; i++)
            {
                double obstacleDistance = ((AngleLock)angleLocks[i]).lockVector.Length;
                if (obstacleDistance > targetVector.Length)
                {
                    angleLocks.RemoveAt(i);
                    i--;
                }
                else if (obstacleDistance < robotCollisionAvoidanceDistance / 2)
                {
                    robot.leftSpeed = 0.0;
                    robot.rightSpeed = 0.0;
                }
            }

            ///join locks
            for (int i = 0; i < angleLocks.Count; i++)
            {
                AngleLock angleLock = (AngleLock)angleLocks[i];
                if (angleLock.lockVector.Length > targetVector.Length)
                {
                    angleLocks.RemoveAt(i);
                    i--;
                }
                for (int j = i + 1; j < angleLocks.Count; j++)
                {
                    if (angleLock.TryJoin((AngleLock)angleLocks[j]))
                    {
                        angleLocks.RemoveAt(j);
                        j--;
                    }
                }
            }

            // find and go around
            double newAngle = robotAngle;
            foreach (AngleLock angleLock in angleLocks)
            {
                newAngle = angleLock.GetClosestFreeAngle(newAngle);
                if (newAngle != robotAngle)
                {
                    if (angleLock.lockVector.Length < robotCollisionAvoidanceDistance) //prevent keeping motion direction
                    {
                        robot.leftSpeed = 0.0;
                        robot.rightSpeed = 0.0;
                    }
                    return new Point2D(robot.x + angleLock.lockVector.Length * Math.Cos(newAngle),
                        robot.y + angleLock.lockVector.Length * Math.Sin(newAngle));
                }
            }
            return new Point2D(0.0, 0.0);
        }
    }
}
